#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
from __future__ import print_function
import numpy as np
from numpy import (deg2rad)
_k = 1
# 换算比例, 因为下面的参数单位为 mm, 但不利于数值计算,量级差别太大,
# 但利于解析计算,因为解析结果针对的都是位置部分的数值

# Define for the lengths of the arms foots etc
ShoulderOffsetY    = _k*98.0
ElbowOffsetY       = _k*15.0
UpperArmLength     = _k*105.0
ShoulderOffsetZ    = _k*100.0
LowerArmLength     = _k*57.75
HandOffsetX        = _k*55.95
HandOffsetZ        = _k*12.31
HipOffsetZ         = _k*85.0
HipOffsetY         = _k*50.0
ThighLength        = _k*100.0
TibiaLength        = _k*102.9
FootHeight         = _k*45.11
NeckOffsetZ        = _k*126.5
# 一下相机参数为 V5.0 的相机参数
CameraBottomX      = _k*50.71
CameraBottomZ      = _k*17.74
CameraBottomAngle  = deg2rad(39.7) # 绕Y轴转动角,见相机侧视图
CameraTopX         = _k*58.71
CameraTopZ         = _k*63.64
CameraTopAngle     = deg2rad(1.2) # 绕Y轴转动角,见相机侧视图
# Head Limits
HeadYawHigh        = 2.0857
HeadYawLow         = -2.0857
HeadPitchHigh      = 0.5149
HeadPitchLow       = -0.6720

HeadLow = np.array([HeadYawLow,HeadPitchLow])
HeadHigh = np.array([HeadYawHigh,HeadPitchHigh])

def check_Head_Limits(angles):
    angles = np.array(angles).flatten()
    return (angles > HeadLow).all() and (angles < HeadHigh).all()

# Left Hand limits
LShoulderPitchHigh = 2.0857
LShoulderPitchLow  = -2.0857
LShoulderRollHigh  = 1.3265
LShoulderRollLow   = -0.3142
LElbowYawHigh      = 2.0875
LElbowYawLow       = -2.0875
LElbowRollHigh     = 0.00001 # Aldebaran gives this value (-0.0349f) but the hand can go further
LElbowRollLow      = -1.5446
LWristYawHigh      = 1.8238
LWristYawLow       = -1.8238

LArmLow = np.array([LShoulderPitchLow,LShoulderRollLow,LElbowYawLow,LElbowRollLow,LWristYawLow])
LArmHigh = np.array([LShoulderPitchHigh,LShoulderRollHigh,LElbowYawHigh,LElbowRollHigh,LWristYawHigh])

def check_LArm_Limits(angles):
    angles = np.array(angles).flatten()
    return (angles[:5] > LArmLow).all() and (angles[:5] < LArmHigh).all()

# Right Hand limits
RShoulderPitchHigh = 2.0857
RShoulderPitchLow  = -2.0857
RShoulderRollHigh  = 0.3142
RShoulderRollLow   = -1.3265
RElbowYawHigh      = 2.0875
RElbowYawLow       = -2.0875
RElbowRollHigh     = 1.5446
RElbowRollLow      = 0.00001 # Aldebaran gives this value (0.0349f) but the hand can go further
RWristYawHigh      = 1.8238
RWristYawLow       = -1.8238

RArmLow = np.array([RShoulderPitchLow,RShoulderRollLow,RElbowYawLow,RElbowRollLow,RWristYawLow])
RArmHigh = np.array([RShoulderPitchHigh,RShoulderRollHigh,RElbowYawHigh,RElbowRollHigh,RWristYawHigh])

def check_RArm_Limits(angles):
    angles = np.array(angles).flatten()
    return (angles[:5] > RArmLow).all() and (angles[:5] < RArmHigh).all()

# Left Leg limits
LHipYawPitchHigh   = 0.7408
LHipYawPitchLow    = -1.1453
LHipRollHigh       = 0.7904
LHipRollLow        = -0.3794
LHipPitchHigh      = 0.4840
LHipPitchLow       = -1.7739
LKneePitchHigh     = 2.1125
LKneePitchLow      = -0.0923
LAnklePitchHigh    = 0.9227
LAnklePitchLow     = -1.1895
LAnkleRollHigh     = 0.7690
LAnkleRollLow      = -0.3978

LLegLow = np.array([LHipYawPitchLow,LHipRollLow,LHipPitchLow,LKneePitchLow,LAnklePitchLow,LAnkleRollLow])
LLegHigh = np.array([LHipYawPitchHigh,LHipRollHigh,LHipPitchHigh,LKneePitchHigh,LAnklePitchHigh,LAnkleRollHigh])

def check_LLeg_Limits(angles):
    angles = np.array(angles).flatten()
    return (angles > LLegLow).all() and (angles < LLegHigh).all()

# Left Right limits
RHipYawPitchHigh   = 0.7408
RHipYawPitchLow    = -1.1453
RHipRollHigh       = 0.4147
RHipRollLow        = -0.7383
RHipPitchHigh      = 0.4856
RHipPitchLow       = -1.7723
RKneePitchHigh     = 2.1201
RKneePitchLow      = -0.1030
RAnklePitchHigh    = 0.9320
RAnklePitchLow     = -1.1864
RAnkleRollHigh     = 0.3886
RAnkleRollLow      = -1.1864

RLegLow = np.array([RHipYawPitchLow,RHipRollLow,RHipPitchLow,RKneePitchLow,RAnklePitchLow,RAnkleRollLow])
RLegHigh = np.array([RHipYawPitchHigh,RHipRollHigh,RHipPitchHigh,RKneePitchHigh,RAnklePitchHigh,RAnkleRollHigh])

def check_RLeg_Limits(angles):
    angles = np.array(angles).flatten()
    return (angles > RLegLow).all() and (angles < RLegHigh).all()

def check_Limits(names,angles):
    if names == 'Head':
        return check_Head_Limits(angles)
    elif names == 'LArm':
        return check_LArm_Limits(angles)
    elif names == 'RArm':
        return check_RArm_Limits(angles)
    elif names == 'LLeg':
        return check_LLeg_Limits(angles)
    elif names == 'RLeg':
        return check_RLeg_Limits(angles)
    elif names == 'Body':
        return (check_Head_Limits(angles[0:2]) and
                check_LArm_Limits(angles[2:8]) and
                check_LLeg_Limits(angles[8:14]) and
                check_RLeg_Limits(angles[14:20]) and
                check_RArm_Limits(angles[20:26]))
# Masses defines
# Total mass
TotalMassH25       = (5.182530+0.345)
# Torso
TorsoMass          = 1.04956
TorsoX             = _k*-4.13
TorsoY             = _k*0.09
TorsoZ             = _k*43.42

# Not provided by aldebaran
BatteryMass        = 0.345
BatteryX           = _k*-30.00
BatteryY           = _k*0.00
BatteryZ           = _k*39.00

# Head
HeadYawMass        = 0.06442
HeadYawX           = _k*-0.01
HeadYawY           = _k*0.14
HeadYawZ           = _k*-27.42

HeadPitchMass      = 0.60533
HeadPitchX         = _k*-1.12
HeadPitchY         = _k*0.0
HeadPitchZ         = _k*52.58

# Right Hand
RShoulderPitchMass = 0.06996
RShoulderPitchX    = _k*-1.65
RShoulderPitchY    = _k*26.63
RShoulderPitchZ    = _k*0.14

RShoulderRollMass  = 0.15794
RShoulderRollX     = _k*24.29
RShoulderRollY     = _k*-9.52
RShoulderRollZ     = _k*0.32

RElbowYawMass      = 0.06483
RElbowYawX         = _k*-27.44
RElbowYawY         = _k*0.00
RElbowYawZ         = _k*-0.14

RElbowRollMass     = 0.07778
RElbowRollX        = _k*25.52
RElbowRollY        = _k*-2.81
RElbowRollZ        = _k*0.9

RWristYawMass      = 0.18533
RWristYawX         = LowerArmLength+_k*34.34
RWristYawY         = _k*-0.88
RWristYawZ         = _k*3.08

# Right Leg
RHipYawPitchMass   = 0.07118
RHipYawPitchX      = _k*-7.66
RHipYawPitchY      = _k*12.00
RHipYawPitchZ      = _k*27.16
 
RHipRollMass       = 0.13053
RHipRollX          = _k*-15.49
RHipRollY          = _k*-0.29
RHipRollZ          = _k*-5.16
 
RHipPitchMass      = 0.38976
RHipPitchX         = _k*1.39
RHipPitchY         = _k*-2.25
RHipPitchZ         = _k*-53.74
 
RKneePitchMass     = 0.29163
RKneePitchX        = _k*3.94
RKneePitchY        = _k*-2.21
RKneePitchZ        = _k*-49.38
 
RAnklePitchMass    = 0.13415
RAnklePitchX       = _k*0.45
RAnklePitchY       = _k*-0.3
RAnklePitchZ       = _k*6.84
 
RAnkleRollMass     = 0.16171
RAnkleRollX        = _k*25.42
RAnkleRollY        = _k*-3.32
RAnkleRollZ        = _k*-32.39
